//
// Created by fanghr on 18-3-28.
//

#include <vector>
#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

cv::Mat applyLookUp(const cv::Mat &image, const cv::MatND &lookup) {
    cv::Mat result(image.rows, image.cols, CV_8U);

    cv::Mat_<uchar>::iterator itr = result.begin<uchar>();
    cv::Mat_<uchar>::const_iterator it = image.begin<uchar>();
    cv::Mat_<uchar>::const_iterator itend = image.end<uchar>();
    for (; it != itend; ++it, ++itr) {

        *itr = lookup.at<uchar>(*it);
    }
    return result;
}

int main() {

    VideoCapture cap1(4);
    if (!cap1.isOpened())return -1;

    VideoCapture cap2(0);
    if (!cap2.isOpened())return -1;
    cap1.set(CV_CAP_PROP_FRAME_WIDTH, 50);
    cap2.set(CV_CAP_PROP_FRAME_WIDTH, 50);

    cap1.set(CV_CAP_PROP_FRAME_HEIGHT, 50);
    cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 50);

    Mat img1;
    Mat img2;

    Mat disp, disp8;
    while (1) {
        cap1 >> img1;
        cap2 >> img2;
        cv::StereoSGBM sgbm;
        int SADWindowSize = 9;
        sgbm.preFilterCap = 63;
        sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
        int cn = img1.channels();
        int numberOfDisparities = 512;
        sgbm.P1 = 8 * cn * sgbm.SADWindowSize * sgbm.SADWindowSize;
        sgbm.P2 = 32 * cn * sgbm.SADWindowSize * sgbm.SADWindowSize;
        sgbm.minDisparity = 0;
        sgbm.numberOfDisparities = numberOfDisparities;
        sgbm.uniquenessRatio = 10;
        sgbm.speckleWindowSize = 100;
        sgbm.speckleRange = 32;
        sgbm.disp12MaxDiff = 1;

        int64 t = getTickCount();
        sgbm(img1, img2, disp);
        t = getTickCount() - t;
        cout << "Time elapsed:" << t * 1000 / getTickFrequency() << endl;
        disp.convertTo(disp8, CV_8U, 255 / (numberOfDisparities * 16.));
    }
    int dim(256);
    cv::Mat lut(1,
                &dim,
                CV_8U);

    for (int i = 0; i < 256; i++) {

        lut.at<uchar>(i) = 255 - i;
    }

    cv::namedWindow("Negative image", WINDOW_NORMAL);
    cv::imshow("Negative image", applyLookUp(disp8, lut));
    namedWindow("left", WINDOW_NORMAL);
    imshow("left", img1);
    namedWindow("right", WINDOW_NORMAL);
    imshow("right", img2);
    namedWindow("disparity", WINDOW_NORMAL);
    imshow("disparity", disp8);
    waitKey();
    imwrite("sgbm_disparity.png", disp8);
    cvDestroyAllWindows();
    return 0;
}
